
#include "RBFfunction.h"

RBFfunction::RBFfunction() {
   
   n = 0;
   npolyterms = 0; // 4 or 10
   
   iM = 0;
   lambdas = 0;
   RightSide = 0;
}

RBFfunction::~RBFfunction() {
   
   clear();
}

void RBFfunction::setting(MLS::Constraints *c) {
   
   constraints = *c;
   compute_iM();
}

void RBFfunction::clear() {
   
   constraints.clear();
   n = 0;
   
   gsl_matrix_free(iM);
   gsl_matrix_free(lambdas);
   gsl_matrix_free(RightSide);
}

void RBFfunction::polynomialDegree(uint i) {
   
   switch (i) {
      case 0: npolyterms = 0;  break;
      case 1: npolyterms = 4;  break;
      case 2: npolyterms = 10; break;
   }
   
   //~ compute_iM();
   //~ execute_deformation();
}

inline Number RBFfunction::phi(const Vector3& v) {
   
   const Number &sqrLen = v*v;
   //~ return sqrLen*(sqrt(sqrLen)); // phi(v)=|v|^3
     return std::sqrt(sqrLen);   // phi(v)=|v|
   
//    if (v*v == 0) return 0;
//    return (v*v)*log(sqrt(v*v));
      
//      return 1.0/sqrt(v*v+1);
//      return sqrt(v*v);

   //~ float h=1.0;
   //~ return exp(-(v*v)/(h*h));
}

/// It computes the matrix inverse
void RBFfunction::compute_iM() {
   
   if (constraints.empty())  return;
   
   n = constraints.size();
   
   /// Built the RBF matrix 
   uint size = n+npolyterms;
   gsl_matrix *M = gsl_matrix_alloc(size, size);
   for (uint row=0; row<n; ++row) {
      const Point3 Pf = constraints.getPosition(row);
      for (uint col=0; col<n; ++col) {
         const Point3 Pc = constraints.getPosition(col);
         gsl_matrix_set(M,row,col,phi(Pf-Pc));
      }
      for (uint col=n; col<size; ++col) {
         gsl_matrix_set(M,row,col,polynomial(Pf,col-n));
      }
   }
   
   /// Fill the bottom rows of the matrix
   for (uint row=n; row<size; ++row) {
      for (uint col=0; col<size; ++col) {
         if (col < n) gsl_matrix_set(M,row,col,gsl_matrix_get(M,col,row));
         else         gsl_matrix_set(M,row,col,0.0);
      }
   }
   
   for (uint row=0; row<size; ++row) {
      cout<<endl;
      for (uint col=0; col<size; ++col) {
         cout<<" "<<gsl_matrix_get(M,col,row);
      }
   }
   
   /// Invert the matrix
   if (iM != 0)  gsl_matrix_free(iM);
   iM = gsl_matrix_alloc(size, size);
   bool flag = invert(M->data,iM->data,size);
   assert(flag);
   
   cout<<"the inverse"<<endl;
   for (uint row=0; row<size; ++row) {
      cout<<endl;
      for (uint col=0; col<size; ++col) {
         cout<<" "<<gsl_matrix_get(iM,col,row);
      }
   }
   
   cout<<"os Ps"<<endl;
   for (uint i=0; i<n; ++i) {
      const Point3 &P =constraints.getPosition(i); /// ????????? modified constraints
      cout<<"P: "<<P[0]<<" "<<P[1]<<" "<<P[2]<<endl;
   }
   
   /// Allocate memory
   if (RightSide != 0)  gsl_matrix_free(RightSide);
   if (lambdas != 0)    gsl_matrix_free(lambdas);
   RightSide = gsl_matrix_alloc(size, 3);
   lambdas = gsl_matrix_alloc(size, 3);
}

/// It computes the RBF's weights 
void RBFfunction::compute_lambdas(MLS::Constraints *c) {
   
   if (c == 0)     return;
   if (c->empty()) return;
   
   //~ cout<<"computing the weights"<<endl;
   
   //~ cout<<"os Qs"<<endl;
   for (uint i=0; i<n; ++i) {
      const Point3 &Q = c->getPosition(i); /// ????????? modified constraints
      gsl_matrix_set(RightSide,i,0,Q[0]);
      gsl_matrix_set(RightSide,i,1,Q[1]);
      gsl_matrix_set(RightSide,i,2,Q[2]);
      //~ cout<<"Q: "<<Q[0]<<" "<<Q[1]<<" "<<Q[2]<<endl;
   }
   
   /// additional restrictions initialization
   for (uint i=0; i<npolyterms; ++i) {
      gsl_matrix_set(RightSide,n+i,0,0.0);
      gsl_matrix_set(RightSide,n+i,1,0.0);
      gsl_matrix_set(RightSide,n+i,2,0.0);
   }
   
   matvecmult(iM->data, RightSide->data, lambdas->data, n+npolyterms);
   
   cout<<"the RightSide"<<endl;
   for (uint i=0; i<n+npolyterms; ++i) {
      cout<<" "<<gsl_matrix_get(RightSide,i,0)<<" "<<gsl_matrix_get(RightSide,i,1)<<" "<<gsl_matrix_get(RightSide,i,2)<<endl;
   }
   
   cout<<"the lambdas"<<endl;
   for (uint i=0; i<n+npolyterms; ++i) {
      cout<<" "<<gsl_matrix_get(lambdas,i,0)<<" "<<gsl_matrix_get(lambdas,i,1)<<" "<<gsl_matrix_get(lambdas,i,2)<<endl;
   }
}

Point3 RBFfunction::evalf(const Point3 &p) {
   
   Number rx=0, ry=0, rz=0;
   
   /// radial component
   for (uint i=0; i<n; ++i) {
      const Point3 &P = constraints.getPosition(i);
      const Number &PHI = phi(p-P);
      rx += gsl_matrix_get(lambdas,i,0)*PHI;
      ry += gsl_matrix_get(lambdas,i,1)*PHI;
      rz += gsl_matrix_get(lambdas,i,2)*PHI;
   }
   
   /// polynomial component
   /// parte das colunas de lambdas sao os coeficientes do polinomio
   /// que corresponde ao componente x, y ou z
   for (int i=0; i<npolyterms; ++i) {
      const Number &valpoly = polynomial(p, i);
      rx += gsl_matrix_get(lambdas,n+i, 0)*valpoly;
      ry += gsl_matrix_get(lambdas,n+i, 1)*valpoly;
      rz += gsl_matrix_get(lambdas,n+i, 2)*valpoly;
   }
   
   return Point3(rx, ry, rz);
}

Point3 RBFfunction::evalf(const Vector3 &v) {
   
   return evalf(Point3(v[0],v[1],v[2]));
}

Number RBFfunction::polynomial(const Point3& p, uint i) {
   
   assert(i<10);
   Number value=0;
   switch(i) {
      case 0:  value=1;               break;
      case 1:  value=p[0];            break;
      case 2:  value=p[1];            break;
      case 3:  value=p[2];            break;
      case 4:  value=p[0]*p[1];       break;
      case 5:  value=p[0]*p[2];       break;
      case 6:  value=p[1]*p[2];       break;
      case 7:  value=p[0]*p[0];       break;
      case 8:  value=p[1]*p[1];       break;
      case 9:  value=p[2]*p[2];       break;
   }
   return value;
}

/// esta funcion deberia estar fuera de esta clase ... para q la clase sea independiente de lo q interpole
/// la deformacion puede ser aplicada a los vertices de la malla o a los p* o a cualquier otra cosa
//~ void RBFfunction::execute_deformation() {
   
   /*
   if (constraints == 0 or model== 0 ) return;
   if (constraints->empty()) return;
   
   compute_lambdas();
   
   Vertex_iterator vi = model->vertices_begin();
   for (unsigned i=0; i<model->size_of_vertices(); ++i) {
      //~ if (vi[i].getType() == 'C')  continue;
      //~ if (vi[i].getType() == 'F')  continue;
      //~ vi[i].pointDef(evalf(vi[i].point()));
      
      float *pp = &model->vpositions[i*3];
      Point3 p(pp[0], pp[1], pp[2]);
      p = evalf(p);
      pp = &model->vpositionsDef[i*3];
      pp[0] = p[0];  // x
      pp[1] = p[1];  // y
      pp[2] = p[2];  // z
      vi[i].pointDef(p);            // precissa por causa de ser necesario renderizar las restricciones y calcular las normales
   }
   */
//~ }

// Point3 RBFfunction::gradf(const Point3 &p) {
// 	
// 	Number x = p.x(); 
// 	Number y = p.y();  
// 	Number z = p.z();
// 	cout<<x<<" "<<y<<" "<<z<<endl;
// 	int n = constraint.size();
// 	Number gfx = p1;
// 	Number gfy = p2;
// 	Number gfz = p3;
// 	for (int i = 0; i < n; ++i) {
// 		Number rx = x-constraint[i].x();
// 		Number ry = y-constraint[i].y();
// 		Number rz = z-constraint[i].z();
// 		Number nr = std::sqrt(rx*rx + ry*ry + rz*rz);
// 	
// 		gfx = gfx + (constraint[i].w())*(3*nr)*rx;
// 		gfy = gfy + (constraint[i].w())*(3*nr)*ry;
// 		gfz = gfz + (constraint[i].w())*(3*nr)*rz;
// 	}
// 	Number ngf = std::sqrt(gfx*gfx + gfy*gfy + gfz*gfz);
// 	assert (ngf!=0);
// 	return Point3(gfx/ngf,gfy/ngf,gfz/ngf);
// 	return Point3(gfx,gfy,gfz);
// }

